#include "libutils.h"
#include "protocol.h"

void make_data376_down(Datagram *dst, __u8 *userdata, int userdatalen,
	__u8 routerID, __u8 affiliatedNodeID, __u8 commModuleID, __u8 collisionDetection,
	__u8 relayLevel, __u8 channelID, __u8 errorCorrectionCodeID, __u8 responseBytes,
	__u16 commBaudRate, __u16 rateUnitID,
	__u8 *sourceAddr, __u8 *relayAddr, __u8 *destinationAddr,
	__u8 AFN, __u8 DT1, __u8 DT2)
{
	dst->prefix->start = 0x68;
	dst->prefix->len = userdatalen + 6;

	if(sourceAddr)
		dst->prefix->len += 6;
	if(relayAddr)
		dst->prefix->len += 6;
	if(destinationAddr)
		dst->prefix->len += 6;

	/* C ontrol */
	dst->prefix->control.DIR = 0;
	dst->prefix->control.PRM = 1;
	dst->prefix->control.commMode = 0x20;

	/* R */
	dst->prefix->info.info_down.routerID = routerID;
	dst->prefix->info.info_down.affiliatedNodeID = affiliatedNodeID;
	dst->prefix->info.info_down.commModuleID = commModuleID;
	dst->prefix->info.info_down.collisionDetection = collisionDetection;
	dst->prefix->info.info_down.relayLevel = relayLevel;
	dst->prefix->info.info_down.channelID = channelID;
	dst->prefix->info.info_down.errorCorrectionCodeID = errorCorrectionCodeID;
	dst->prefix->info.info_down.responseBytes = responseBytes;
	dst->prefix->info.info_down.commBaudRate = commBaudRate;
	dst->prefix->info.info_down.rateUnitID = rateUnitID;
	
	/* A address */
	if(sourceAddr)
		memcpy(dst->packet_addr.sourceAddr, sourceAddr, 6);
	if(relayAddr)
		memcpy(dst->packet_addr.relayAddr, relayAddr, 6);
	if(destinationAddr)
		memcpy(dst->packet_addr.destinationAddr, destinationAddr, 6);

	/* AFN-DT1-DT2 */
	dst->infix->AFN = AFN;
	dst->infix->DT1 = DT1;
	dst->infix->DT2 = DT2;

	if(userdata)
		memcpy(dst->userData, userdata, userdatalen);

	//dst->postfix->checkSum = get_cs((__u8*)(dst + 3), dst->prefix->len - 6);
	dst->postfix->checkSum = 0;
	dst->postfix->end = 0x16;
}

/* init 
** AFN=01H F1/F2/F3
*/
void make_data376_init(Datagram *dst, __u8 *userdata, int userdatalen,
	__u8 responseBytes, __u8 *sourceAddr, __u8 *destinationAddr,
	__u8 AFN, __u8 fn)
{
	__u8 DT1 = 0;
	__u8 DT2 = 0;

	SetFn(fn, &DT1, &DT2);
	make_data376_down(dst, userdata, userdatalen,
		0, 0, 0x20, 0,
		0, 0, 0, responseBytes,
		0, 0,
		sourceAddr, NULL, destinationAddr,
		0x01, DT1, DT2);
		
}

/* query 
** AFN=03H F1
*/
void make_data376_query(Datagram *dst, __u8 *userdata, int userdatalen,
	__u8 responseBytes, __u8 *sourceAddr, __u8 *destinationAddr,
	__u8 AFN, __u8 fn)
{
	__u8 DT1 = 0;
	__u8 DT2 = 0;
	
	SetFn(fn, &DT1, &DT2);
	make_data376_down(dst, userdata, userdatalen, 
		0, 0, 0x20, 0,
		0, 0, 0, responseBytes,
		0, 0,
		sourceAddr, NULL, destinationAddr,
		0x03, DT1, DT2);

}

/* control
** AFN=05H F1
*/
void make_data376_control(Datagram *dst, __u8 *userdata, int userdatalen,
	__u8 responseBytes, __u8 *sourceAddr, __u8 *destinationAddr,
	__u8 AFN, __u8 fn)
{
	__u8 DT1 = 0;
	__u8 DT2 = 0;
	
	SetFn(fn, &DT1, &DT2);
	make_data376_down(dst, userdata, userdatalen,
		0, 0, 0x20, 0,
		0, 0, 0, responseBytes,
		0, 0,
		sourceAddr, NULL, destinationAddr,
		0x05, DT1, DT2);
}

/* read
** AFN=06H F1 F2
*/
void make_data376_read(Datagram *dst, __u8 *userdata, int userdatalen, int *lenresult,
	__u8 responseBytes, __u8 *sourceAddr, __u8 *destinationAddr,
	__u8 AFN, __u8 fn)
{
	__u8 DT1 = 0;
	__u8 DT2 = 0;
	
	SetFn(fn, &DT1, &DT2);
	make_data376_down(dst, userdata, userdatalen,
		0, 0, 0x20, 0,
		0, 0, 0, responseBytes,
		0, 0,
		sourceAddr, NULL, destinationAddr,
		0x06, DT1, DT2);

}

void make_data376_up(Datagram *dst, __u8 *userdata, int userdatalen,
	__u8 routerID, __u8 commModuleID, __u8 relayLevel, __u8 channelID, 
	__u8 phaseLineMark, __u8 meterChannelFeature, __u8 lastCommandSignalQuality, 
	__u8 lastResponseSignalQuality,
	__u8 *sourceAddr, __u8 *relayAddr, __u8 *destinationAddr,
	__u8 AFN, __u8 DT1, __u8 DT2)
{
	dst->prefix->start = 0x68;
	dst->prefix->len = userdatalen + 6;
	
	/* C ontrol */
	dst->prefix->control.DIR = 1;
	dst->prefix->control.PRM = 0;
	dst->prefix->control.commMode = 0x20;

	dst->prefix->info.info_up.routerID = routerID;
	dst->prefix->info.info_up.no_use1 = 0;
	dst->prefix->info.info_up.commModuleID = commModuleID;
	dst->prefix->info.info_up.no_use2 = 0;
	dst->prefix->info.info_up.relayLevel = relayLevel;
	dst->prefix->info.info_up.channelID = channelID;
	dst->prefix->info.info_up.no_use3 = 0;
	dst->prefix->info.info_up.phaseLineMark = phaseLineMark;
	dst->prefix->info.info_up.meterChannelFeature = meterChannelFeature;
	dst->prefix->info.info_up.lastCommandSignalQuality = lastCommandSignalQuality;
	dst->prefix->info.info_up.lastResponseSignalQuality = lastResponseSignalQuality;

	/* A address */
	if(sourceAddr)
		memcpy(dst->packet_addr.sourceAddr, sourceAddr, 6);
	if(relayAddr)
		memcpy(dst->packet_addr.relayAddr, relayAddr, 6);
	if(destinationAddr)
		memcpy(dst->packet_addr.destinationAddr, destinationAddr, 6);

	/* AFN-DT1-DT2 */
	dst->infix->AFN = AFN;
	dst->infix->DT1 = DT1;
	dst->infix->DT2 = DT2;

	if(userdata)
		memcpy(dst->userData, userdata, userdatalen);

	dst->postfix->checkSum = get_cs((__u8*)(dst + 3), dst->prefix->len - 6);
	dst->postfix->end = 0x16;
}

Datagram * ParseDatagram(__u8 *packet, __u16 len)
{
	__u8 sum = 0;
    Datagram * dg = (Datagram *)calloc(1, sizeof(Datagram) + len);
    for (int i = 0; i < len; i++)
        if (packet[0] == 0x3f)
            packet++;
        else
            break;
    if (packet[0] == 0x68)
        dg->packet_status |= PACKET_VALID_START;
    dg->len = *(__u16 *)(packet + 1);
    if (dg->len <= len)
        dg->packet_status |= PACKET_VALID_LEN;
    if (dg->packet_status & PACKET_VALID_LEN)
    {
        if (packet[dg->len - 1] == 0x16)
            dg->packet_status |= PACKET_VALID_END;
        if (dg->packet_status & PACKET_VALID_END)
        {
            for (int i = 3; i < dg->len - 2; i++)
                sum += packet[i];
            if (sum == packet[dg->len - 2])
                dg->packet_status |= PACKET_VALID_CHECKSUM;
        }
    }
    if (~dg->packet_status & PACKET_VALID)
        return dg;
    memcpy(dg->packet, packet, len);

    dg->prefix = (packet_prefix *)dg->packet;

    if (dg->prefix->info.info_down.commModuleID == 0)
    {
        dg->packet_addr.destinationAddr = NULL;
        dg->packet_addr.relayAddr = NULL;
        dg->packet_addr.sourceAddr = NULL;
    }
    else
    {
        if (dg->prefix->control.DIR == 0)
        {
            dg->packet_addr.sourceAddr = (__u8 *)dg->prefix + 10 /* sizeof(packet_prefix) */; 
            if (dg->prefix->info.info_down.relayLevel != 0)
            {
                dg->packet_addr.relayAddr = dg->packet_addr.sourceAddr + 6;
                dg->packet_addr.destinationAddr = dg->packet_addr.relayAddr + 6 * dg->prefix->info.info_down.relayLevel;
            }
            else
            {
                dg->packet_addr.relayAddr = NULL;
                dg->packet_addr.destinationAddr = dg->packet_addr.sourceAddr + 6;
            }
        }
        else
        {
            dg->packet_addr.sourceAddr = (__u8 *)dg->prefix + sizeof(packet_prefix); 
            dg->packet_addr.relayAddr = NULL;
            dg->packet_addr.destinationAddr = dg->packet_addr.sourceAddr + 6;
        }
    }

    dg->infix = (packet_infix *)(&(dg->prefix->info.info_up.reserve) + 1);
    if (dg->packet_addr.destinationAddr != 0)
        dg->infix = (packet_infix *)((__u8 *)dg->packet_addr.destinationAddr + 6);

    dg->userData = (__u8 *)dg->infix + sizeof(packet_infix);

    if (dg->packet[dg->len - 1] == 0x16)
        dg->postfix = (packet_postfix *)((__u8 *)dg->packet + len - 2);
    else
        dg->postfix = NULL;

    if (dg->postfix == NULL)
        dg->userDataLen = dg->packet + dg->len - dg->userData;
    else
        dg->userDataLen = (__u8 *)dg->postfix - (__u8 *)dg->userData;
    return dg;
}

__u8* Datagram2raw(Datagram *pdg, __u8 *raw, __u8 *len)
{
	__u8 *p_raw = raw;

	// 拷贝
	memcpy(p_raw, pdg, sizeof(packet_prefix));
	p_raw += sizeof(packet_prefix);

	if(pdg->packet_addr.sourceAddr) {
		memcpy(p_raw, pdg->packet_addr.sourceAddr, 6);
		p_raw += 6;
	}

	if(pdg->packet_addr.relayAddr) {
		memcpy(p_raw, pdg->packet_addr.relayAddr, 6);
		p_raw += 6;
	}

	if(pdg->packet_addr.destinationAddr) {
		memcpy(p_raw, pdg->packet_addr.destinationAddr, 6);
		p_raw += 6;
	}

	memcpy(p_raw, pdg->infix, sizeof(packet_infix));
	p_raw += sizeof(packet_infix);

	if(pdg->userData) {
		memcpy(p_raw, pdg->userData, pdg->userDataLen);
		p_raw += pdg->userDataLen;
	}

	memcpy(p_raw, pdg->postfix, sizeof(packet_postfix));
	*len = p_raw - raw + sizeof(packet_postfix);

	return raw;
}

void show_Datagram(Datagram *pd)
{
	int flag = 0;

	if(pd->packet_status && PACKET_VALID_START == 0) {
		printf("起始标记非法\n");
		flag = 1;
	}
	if(pd->packet_status && PACKET_VALID_LEN == 0){
		printf("长度非法\n");
		flag = 1;
	}
	if(pd->packet_status && PACKET_VALID_CHECKSUM == 0){
		printf("校验和非法\n");
		flag = 1;
	}
	if(pd->packet_status && PACKET_VALID_END == 0){
		printf("结束标记非法\n");
		flag = 1;
	}

	if(flag == 1) {
		printf("非法的376.2数据包\n");
		return;
	}
	
	printf("长度：%02X\n", pd->len);
	puts("控制域：");
	printf("\t传输方向位：%02X\n", (pd->prefix)->control.DIR);
	printf("\t启动标志位：%02X\n", (pd->prefix)->control.PRM);
	printf("\t通信方式：%02X\n", (pd->prefix)->control.commMode);
	
	puts("用户数据：");
	puts("信息域R：");
	//下行报文
	if((pd->prefix)->control.DIR == 0) {
		printf("\t路由标示：%02X\n", (pd->prefix)->info.info_down.routerID);
		printf("\t附属节点表示：%02X\n", (pd->prefix)->info.info_down.affiliatedNodeID);
		printf("\t通信模块表示：%02X\n", 
			(pd->prefix)->info.info_down.commModuleID);
		printf("\t冲突检测：%02X\n",
			(pd->prefix)->info.info_down.collisionDetection);
		printf("\t中继级别：%02X\n",
			(pd->prefix)->info.info_down.relayLevel);
		printf("\t信道标示：%02X\n",
			(pd->prefix)->info.info_down.channelID);
		printf("\t纠错编码表示：%02X\n",
			(pd->prefix)->info.info_down.errorCorrectionCodeID);
		printf("\t预计应答字节数：%02X\n",
			(pd->prefix)->info.info_down.responseBytes);
		printf("\t通信速率：%02X\n",
			(pd->prefix)->info.info_down.commBaudRate);
		printf("\t速率单位标示：%02X\n",
			(pd->prefix)->info.info_down.rateUnitID);
	} else {
		printf("\t路由标示：%02X\n", (pd->prefix)->info.info_up.routerID);
		printf("\t通信模块标示：%02X\n",
			(pd->prefix)->info.info_up.commModuleID);
		printf("\t中继级别：%02X\n", 
			(pd->prefix)->info.info_up.relayLevel);
		printf("\t信道标示：%02X\n",
			(pd->prefix)->info.info_up.channelID);
		printf("\t实测相线标示：%02X\n",
			(pd->prefix)->info.info_up.phaseLineMark);
		printf("\t电表通道特征：%02X\n",
			(pd->prefix)->info.info_up.meterChannelFeature);
		printf("\t末级命令信号品质：%02X\n",
			(pd->prefix)->info.info_up.lastCommandSignalQuality);
		printf("\t末级应答信号品质：%02X\n",
			(pd->prefix)->info.info_up.lastResponseSignalQuality);
	}

	puts("地址域：");

	printf("应用功能码：%02X\n", (pd->infix)->AFN);
	printf("\t数据单元标示：%02x%02X\n", (pd->infix)->DT1,
		(pd->infix)->DT2);
	printf("\t数据单元：");
	mem_dump(pd->userData, pd->userDataLen);
	printf("校验和：%02X\n", (pd->postfix)->checkSum);
}

//获取376.2报文中的645报文
int do_Datagram(Datagram *pd, __u8 **p, __u8 *len)
{
	int flag = 0;

	if(pd->packet_status && PACKET_VALID_START == 0) {
		printf("起始标记非法\n");
		flag = 1;
	}
	if(pd->packet_status && PACKET_VALID_LEN == 0){
		printf("长度非法\n");
		flag = 1;
	}
	if(pd->packet_status && PACKET_VALID_CHECKSUM == 0){
		printf("校验和非法\n");
		flag = 1;
	}
	if(pd->packet_status && PACKET_VALID_END == 0){
		printf("结束标记非法\n");
		flag = 1;
	}

	if(flag == 1) {
		printf("非法的376.2数据包\n");
		return -1;
	}

	if((pd->infix)->AFN == 0x5) {
		//__u8 protocol = pd->userData[0];
		*len = pd->userData[1];
		*p = pd->userData + 2;
	}

	return 0;
}

Packet *ParsePacket(__u8 *data, __u8 len)
{
	Packet *pt;
	if ((pt = (Packet *)malloc(sizeof(Packet) + len)) == NULL)
		return NULL;
	memcpy(pt->packet, data, len);
	pt->packetlen = len;
	pt->addr = &data[1];
	*(__u8 *)&pt->control = data[8];
	pt->datalen = data[9];
	if (pt->datalen)
		pt->data = &data[10];
	else
		pt->data = NULL;
	pt->checksum = *(pt->data + pt->datalen);	
	if (pt->control.direction && pt->control.responseID) // 麓脫脮戮脫娄麓冒虏垄脟脪脦陋脪矛鲁拢脫娄麓冒脮矛
	{
		pt->further.funcode = NULL;
		pt->further.codelen = 0;
		pt->further.has_date = pt->further.has_operator =
		pt->further.has_password = 0;
		pt->further.date = pt->further._operator = 
		pt->further.password = NULL;
		pt->further.datalen = data[9]; // 麓脣路脰脰搂 data[9] == 1
		pt->further.data = &data[10];
	}
	else if (pt->control.direction) // 麓脫脮戮脫娄麓冒虏垄脟脪脦陋脮媒鲁拢脫娄麓冒脮矛
	{
		if (pt->datalen == 0)
		{
			pt->further.funcode = NULL;
			pt->further.codelen = 0;
			pt->further.has_date = pt->further.has_operator =
			pt->further.has_password = 0;
			pt->further.date = pt->further._operator = 
			pt->further.password = NULL;
			pt->further.data = NULL;
			pt->further.datalen = 0;
		}
		else
		{
			if (pt->control.responseID == 0x11
				|| pt->control.responseID == 0x12
				|| pt->control.responseID == 0x13)
			{
				pt->further.funcode = &data[10];
				pt->further.codelen = 4;
				if (pt->datalen > 5)
				{
					pt->further.datalen = pt->datalen - 5;
					pt->further.data = &data[14];
				}
			}
		}
	}
	return pt;
}

void SetFn(__u8 fn, __u8 *pdt1, __u8 *pdt2)
{
	*pdt1 = 1 << ((fn + 7) %8);
	*pdt2 = (fn / 8) - 1;
}

__u8 GetFn(__u8  dt1, __u8 dt2)
{
	int i = 1, j = 0;
	while( !(dt1 & (i << j++)) && (j <= 8));
	return dt1 ? dt2 * 8 + j : 0;
}
